#include <cstdlib>
#include <iostream>
#include <string>
#include <opencv2/aruco.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <opencv2/opencv.hpp>
#include "get_intelligence.h"

/*namespace
{
    const char *about = "Pose estimation of ArUco marker images";
    const char *keys =
        "{d        |16    | dictionary: DICT_4X4_50=0, DICT_4X4_100=1, "
        "DICT_4X4_250=2, DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, "
        "DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8, DICT_6X6_100=9, "
        "DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13, "
        "DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
        "{h        |false | Print help }"
        "{l        |      | Actual marker length in meter }"
        "{v        |<none>| Custom video source, otherwise '0' }"
        "{h        |false | Print help }"
        "{l        |      | Actual marker length in meter }"
        "{v        |<none>| Custom video source, otherwise '0' }";
} // namespace*/
int main()
{
	std::string valueStr;
	size_t v = (size_t)std::stoull(valueStr);
	#if 1
    int wait_time = 10; 
    //路径请更改为视频20200913_201855.mp4的绝对路径
    cv::String videoInput = "../test/20200913_201855.mp4";
    cv::VideoCapture in_video;
    get_inf inf;

    if (videoInput != "0")
    {
        
        if (videoInput.empty())
        {
            return 1;
        }
        char *end = nullptr;
        int source = static_cast<int>(std::strtol(videoInput.c_str(), &end,
                                                  10));
        if (!end || end == videoInput.c_str())
        {
            in_video.open(videoInput); // url
        }
        else
        {
            in_video.open(source); // id
        }
    }
    else
    {
        in_video.open(0);
    }
    if (!in_video.isOpened())
    {
        std::cerr << "failed to open video input: " << videoInput << std::endl;
        return 1;
    }

    cv::VideoWriter vw;
    vw.open("test.avi", 
                        vw.fourcc('M', 'J', 'P', 'G'), 
                        in_video.get(5), 
                        cv::Size(in_video.get(3), in_video.get(4)), 
                        true);

    cv::Mat image;
    cv::Mat camera_matrix, dist_coeffs;
    //路径请更改为相机参数存储calibration_params.yml的绝对路径
    cv::FileStorage fs("../test/calibration_params.yml", cv::FileStorage::READ);

    fs["camera_matrix"] >> camera_matrix;
    fs["distortion_coefficients"] >> dist_coeffs;

    std::ostringstream vector_to_marker;
    while (in_video.grab())
    {
        in_video.retrieve(image);
        inf.weset(image, camera_matrix, dist_coeffs);
        double c_tenengrad = inf.weget_sharp();
        std::string xv = inf.weget_xv();
        std::string yv = inf.weget_yv();
        std::string ic = inf.weget_ic();
        vector_to_marker.str(std::string());
        vector_to_marker << std::setprecision(4)
                         << "Sharpness: " << c_tenengrad;
        cv::putText(image, vector_to_marker.str(),
                    cvPoint(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.6,
                    cvScalar(0, 252, 124), 1, cv::LINE_AA);
        vector_to_marker.str(std::string());
        vector_to_marker << std::setprecision(4)
                         << "Isclear: " << ic;
        cv::putText(image, vector_to_marker.str(),
                    cvPoint(10, 50), cv::FONT_HERSHEY_SIMPLEX, 0.6,
                    cvScalar(0, 252, 124), 1, cv::LINE_AA);
        vector_to_marker.str(std::string());
        vector_to_marker << std::setprecision(4)
                         << "x-variation: " << xv;
        cv::putText(image, vector_to_marker.str(),
                    cvPoint(10, 70), cv::FONT_HERSHEY_SIMPLEX, 0.6,
                    cvScalar(0, 252, 124), 1, cv::LINE_AA);
        vector_to_marker.str(std::string());
        vector_to_marker << std::setprecision(4)
                         << "y-variation: " << yv;
        cv::putText(image, vector_to_marker.str(),
                    cvPoint(10, 90), cv::FONT_HERSHEY_SIMPLEX, 0.6,
                    cvScalar(0, 252, 124), 1, cv::LINE_AA);

        cv::imshow("Pose estimation", image);
        char key = (char)cv::waitKey(wait_time);
        if (    key == 27)
                {break;}
        else if (key==32)
        {
            cv::waitKey(0);
        }
        vw << image;
    }
	#endif
}